package com.zige.zige.view.panoplayer;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;
import android.view.Display;
import android.view.Surface;
import android.view.WindowManager;

import java.util.concurrent.TimeUnit;

import com.zige.zige.view.panoplayer.sensor.DeviceSensorLooper;
import com.zige.zige.view.panoplayer.sensor.EKF.OrientationEKF;
import com.zige.zige.view.panoplayer.sensor.EKF.Vector3d;
import com.zige.zige.view.panoplayer.sensor.SensorEventProvider;


/**
 * Created by admin on 16/1/5.
 */
public class MotionTracker implements SensorEventListener {
    private final Display display;
    private final float[] ekfToHeadTracker = new float[16];
    private final float[] sensorToDisplay = new float[16];
    private float displayRotation = -1.0f;
    private final float[] tmpHeadView = new float[16];
    private final float[] tmpHeadView2 = new float[16];
    private volatile boolean tracking;
    private final OrientationEKF tracker;
    private SensorEventProvider sensorEventProvider;
    private long latestGyroEventClockTimeNs;
    private volatile boolean firstGyroValue = true;
    private float[] initialSystemGyroBias = new float[3];
    private final Vector3d latestGyro = new Vector3d();
    private final Vector3d latestAcc = new Vector3d();

    public static MotionTracker createFromContext(Context context) {
        SensorManager sensorManager = (SensorManager)context.getSystemService(Context.SENSOR_SERVICE);
        Display display = ((WindowManager)context.getSystemService(Context.WINDOW_SERVICE)).getDefaultDisplay();
        return new MotionTracker(new DeviceSensorLooper(sensorManager), display);
    }

    public MotionTracker(SensorEventProvider sensorEventProvider, Display display) {
        this.sensorEventProvider = sensorEventProvider;
        this.tracker = new OrientationEKF();
        this.display = display;
    }

    public void onSensorChanged(SensorEvent event) {
        if(event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            this.latestAcc.set((double)event.values[0], (double)event.values[1], (double)event.values[2]);
            this.tracker.processAcc(this.latestAcc, event.timestamp);

        } else if(event.sensor.getType() == Sensor.TYPE_GYROSCOPE
                || event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) {

            this.latestGyroEventClockTimeNs = System.nanoTime();
            if(event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) {
                if(this.firstGyroValue && event.values.length == 6) {
                    this.initialSystemGyroBias[0] = event.values[3];
                    this.initialSystemGyroBias[1] = event.values[4];
                    this.initialSystemGyroBias[2] = event.values[5];
                }

                this.latestGyro.set((double)(event.values[0] - this.initialSystemGyroBias[0]), (double)(event.values[1] - this.initialSystemGyroBias[1]), (double)(event.values[2] - this.initialSystemGyroBias[2]));
            } else {
                this.latestGyro.set((double)event.values[0], (double)event.values[1], (double)event.values[2]);
            }

            this.firstGyroValue = false;


            this.tracker.processGyro(this.latestGyro, event.timestamp);
        }

    }

    public void onAccuracyChanged(Sensor sensor, int accuracy) {
    }

    public void startTracking() {
        if(!this.tracking) {
            this.tracker.reset();
            this.firstGyroValue = true;
            this.sensorEventProvider.registerListener(this);
            this.sensorEventProvider.start();
            this.tracking = true;
        }
    }

    public void stopTracking() {
        if(this.tracking) {
            this.sensorEventProvider.unregisterListener(this);
            this.sensorEventProvider.stop();
            this.tracking = false;
        }
    }

    public void getLastHeadView(float[] headView, int offset) {
        if(offset + 16 > headView.length) {
            throw new IllegalArgumentException("Not enough space to write the result");
        } else {
            float rotation = 0.0F;
            switch(this.display.getRotation()) {
                case Surface.ROTATION_0:
                    rotation = 0.0F;
                    break;
                case Surface.ROTATION_90:
                    rotation = 90.0F;
                    break;
                case Surface.ROTATION_180:
                    rotation = 180.0F;
                    break;
                case Surface.ROTATION_270:
                    rotation = 270.0F;
            }

            if(rotation != this.displayRotation) {
                this.displayRotation = rotation;
                Matrix.setRotateEulerM(this.sensorToDisplay, 0, 0.0F, 0.0F, -rotation);
                Matrix.setRotateEulerM(this.ekfToHeadTracker, 0, -90.0F, 0.0F, rotation);
            }

            synchronized(this.tracker) {
                if(!this.tracker.isReady()) {
                    return;
                }

                double secondsSinceLastGyroEvent = (double) TimeUnit.NANOSECONDS.toSeconds(System.nanoTime() - this.latestGyroEventClockTimeNs);
                double secondsToPredictForward = secondsSinceLastGyroEvent + 0.057999998331069946D;
                double[] mat = this.tracker.getPredictedGLMatrix(secondsToPredictForward);

                for(int i = 0; i < mat.length; ++i) {
                    this.tmpHeadView[i] = (float)mat[i];
                }
            }

            Matrix.multiplyMM(this.tmpHeadView2, 0, this.sensorToDisplay, 0, this.tmpHeadView, 0);

            Matrix.multiplyMM(headView, offset, this.tmpHeadView2, 0, this.ekfToHeadTracker, 0);
        }
    }


}
